#include <p18cxxx.h>
#include "maindefs.h"
#ifndef __XC8
#include <usart.h>
#else
#include <plib/usart.h>
#endif
#include "my_uart.h"
#include "my_leds.h"
#include "my_i2c.h"

static uart_comm *uc_ptr;

static uart_comm *uc_tx_ptr;

void enableUart(void) {
    RCSTAbits.SPEN = 1;
    TRISCbits.TRISC7 = 1;
    TRISCbits.TRISC6 = 0;
    TXSTAbits.SYNC = 0;

}

void uart_send_int_handler() {
#ifndef __USE18F26J50
    if(uc_tx_ptr->bufptr == uc_tx_ptr->buflen)
    {
        //done transmitting current message, check for new
        unsigned char msgtype;

        unsigned char len  = FromMainLow_recvmsg(MSGLEN, &msgtype, uc_tx_ptr->buffer);

        uc_tx_ptr->bufptr = 0;
        if(len == MSGQUEUE_EMPTY || msgtype != MSGT_UART_TRANSMIT)
        {
            //if no data, disable interrupt
            uc_tx_ptr->buflen = 0;
            PIE1bits.TXIE = 0;
            return;
        }
        else
            uc_tx_ptr->buflen = len;

    }
    if(!BusyUSART())
    {
        WriteUSART(uc_tx_ptr->buffer[uc_tx_ptr->bufptr]);
        uc_tx_ptr->bufptr++;
    }
    
#endif
}

void uart_send(unsigned char length, unsigned char *msg){
    //send message to queue for uart tx handler
    FromMainLow_sendmsg(length, MSGT_UART_TRANSMIT, (void*) msg);
    
    //enable and trigger transmit interrupt
    PIE1bits.TXIE = 1;

}

void send_to_main()
{
    ToMainLow_sendmsg(uc_ptr->buflen, MSGT_UART_DATA, (void *) uc_ptr->buffer);
    uc_ptr->buflen=0;
}

void uart_recv_int_handler() {
    if (DataRdyUSART()) {
        unsigned char c = RCREG, addr = 0, msgType = 0, length = 0;
        if(c==0xFF)
        {
            uc_ptr->buflen = 0;
        }
        else
        {
            uc_ptr->buflen++;
            length = uc_ptr->buflen;
            uc_ptr->buffer[length] = c;
            msgType = uc_ptr->buffer[0];

            switch(msgType)
            {
                case MSGT_GET_SENSOR_DATA:
                {
                    if(length == GET_SENSOR_DATA_LENGTH)
                    {
                        send_to_main();
                    }
                    break;
                }

                case MSGT_GET_ENCODER_DATA:
                {
                    if(length == GET_ENCODER_DATA_LENGTH)
                    {
                        send_to_main();
                    }
                    break;
                }

                case MSGT_SET_MOTOR_DATA:
                {
                    if(length == SET_MOTOR_DATA_LENGTH)
                    {
                        send_to_main();
                    }
                    break;
                }


            }
        }
    if (USART_Status.OVERRUN_ERROR == 1) {
        // we've overrun the USART and must reset
        // send an error message for this
        RCSTAbits.CREN = 0;
        RCSTAbits.CREN = 1;
        ToMainLow_sendmsg(0, MSGT_OVERRUN, (void *) 0);
    }
}
}

void init_uart(uart_comm *uc, uart_comm *uc_tx) {
    uc_ptr = uc;
    uc_tx_ptr = uc_tx;
    uc_ptr->buflen = 0;
    uc_tx->buflen = 0;
    uc_tx->bufptr = 0;
}